void processCmd(char cmdChar) {
  
  switch (cmdChar) {
    case 'a': {
      sendAccel();
      break;
    }
    case 'g': {
      sendGyro();
      break;
    }
    case 'h': {
      sendAlt();
      break;
    }
    case 'i': {
      sendIMU();
      break;
    }
    case 'l': {
      sendGPS();
      break;
    }
    case 'p': {
      sendPower();
      break;
    }
    case 'r': {
      //MsTimer2::stop();
      resetINU();
      //MsTimer2::start();
      break;
    }
    case 's': {
      sendAccel();
      sendGyro();
      sendAlt();
      sendGPS();
      sendPower();
      sendIMU();
      break;
    }
    case 't': {
      int mtr = osc.iPop();
      int cmd = osc.iPop();
      constrain(cmd, SERVO_MIN, SERVO_MAX);
      switch (mtr) {
        case 1:
        {
          servo1.writeMicroseconds(cmd);
          servo1_cmd = cmd;
          break;
        }
        case 2:
        {
          servo2.writeMicroseconds(cmd);
          servo2_cmd = cmd;
          break;
        }
        case 3:
        {
          servo3.writeMicroseconds(cmd);
          servo3_cmd = cmd;
          break;
        }
        case 4:
        {
          servo4.writeMicroseconds(cmd);
          servo4_cmd = cmd;
          break;
        }
      }
      break;
    }
  }
}
